#include "servoitf.h"
#include "jkservo.h"
#include "cia402.h"
#include "log/zlog.hh"
#include "rtbus/rtbus.h"
#include "jkutil/configparser.h"
#include "config.h"

#define ABS(x) x > 0 ? x : -x
using rtdev::ServoItf;
std::shared_ptr<ServoItf> rtdev::create_servo(std::string busname)
{
    auto bus = rtbus::BusMgr::get()->bus(busname);
    if (bus == nullptr)
    {
        return nullptr;
    }
    if (bus->is_can())
    {
        return std::make_shared<rtdev::JkServo>();
    }
    else
    {
        return std::make_shared<rtdev::Cia402>();
    }
    return nullptr;
}

ServoItf::ServoItf()
{
    sim_flag_ = 0;
    alive_watchdog_ = 0;
    alive_flag_ = false;
    last_alive_cnt_ = 0;
    memset(&cmd_, 0, sizeof(cmd_));
    memset(&stat_, 0, sizeof(stat_));
    memset(&cfg_, 0, sizeof(cfg_));
}
rtdev::ServoItf::~ServoItf() {}

int rtdev::ServoItf::init(std::string bus_name, int devid, int joint_id)
{
    cfg_.bus = bus_name;
    cfg_.devid = devid;
    bind_bus(bus_name, devid);
    cfg_.joint_id = joint_id;
    cmd_.opmode = 8;  // 默认位置模式
    reset();
    return 0;
}
void rtdev::ServoItf::set_sim(bool sim) { sim_flag_ |= sim; }
int rtdev::ServoItf::reset()
{
    Util::ConfigParser p(ZUC_DEFAULT_ROOT_DIR "/configs/default/rtdev.xml");
    auto servo_path = Util::ConfigParser::str_append_index("servo", cfg_.joint_id);
    bool simflag_cfg = false;
    p.get(servo_path + ".sim", simflag_cfg, 1);
    set_sim(simflag_cfg);
    return 0;
}

int ServoItf::bind_bus(std::string bus_name, int devid)
{
    cfg_.bus = bus_name;
    cfg_.devid = devid;
    bus_ = rtbus::BusMgr::get()->bus(bus_name);

    return 0;
}
int rtdev::ServoItf::set_opmode(TRACKING_MODE mode)
{
    switch (mode)
    {
    case TRACKING_MODE::OPMODE_CSP:
        cmd_.opmode = 8;
        break;
    case TRACKING_MODE::OPMODE_CST:
        cmd_.opmode = 10;
        break;
    case TRACKING_MODE::OPMODE_HOME:
        cmd_.opmode = 6;
        break;
    default:
        cmd_.opmode = 0;
    }

    return 0;
}

int ServoItf::set_position(double pos)
{
    cmd_.target_pos = (int32_t)pos;
    return 0;
}
double ServoItf::get_position(bool is_cmd)
{
    if (is_sim())
    {
        return cmd_.target_pos;
    }
    return is_cmd ? cmd_.target_pos : stat_.act_pos;
}
int ServoItf::set_torq_offset(int16_t torq_off)
{
    cmd_.torq_offset = torq_off;
    return 0;
}
int ServoItf::set_torq(double torq)
{
    cmd_.target_torq = (int32_t)torq;
    return 0;
}
double ServoItf::get_torq() { return stat_.act_torq; }
std::vector<uint32_t> ServoItf::get_errorcode()
{
    std::vector<uint32_t> ret;
    return ret;
}
int ServoItf::set_controlword(uint16_t cw)
{
    cmd_.controlword = cw;
    return 0;
}
uint16_t ServoItf::get_controlword() { return cmd_.controlword; }
uint16_t ServoItf::get_statusword() { return stat_.statusword; }

uint8_t rtdev::ServoItf::get_opmode_display() { return stat_.opmode_display; }

void ServoItf::check_alive()
{
    bool alive = false;
    if (sim_flag_)
    {
        alive = true;
    }
    else
    {
        if (alive_watchdog_ <= 0)
        {
            alive = false;
        }
        else
        {
            alive_watchdog_--;
            alive = true;
        }
    }
    if (alive != alive_flag_)
    {
        // zlog()->info("Device: Servo{} is {}, sim={}", id_, alive ? "online" : "offline", sim_flag_);
        alive_flag_ = alive;
    }
}
int ServoItf::keep_alive(int alive)
{
    if (last_alive_cnt_ != alive)
    {
        last_alive_cnt_ = alive;
        alive_watchdog_ = 5;
        return 0;
    }
    return -1;
}
bool ServoItf::is_alive()
{
    if (is_sim())
        return true;
    return alive_flag_;
}
bool ServoItf::is_sim() { return sim_flag_ == 1; }

void rtdev::ServoItf::set_home()
{
    cmd_.home = 1;
    stat_.home = false;
}

bool rtdev::ServoItf::is_homing() { return stat_.opmode_display == 6; }

bool rtdev::ServoItf::is_homed() { return stat_.home; }
